/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Gyro;

/**
 *
 * @author Team 1533
 */
public class PositionAccumulator {

	private Gyro gyro; // added STJ to use gyro measurement instead of encoders for angle
	private Encoder left, right;
	private float trackWidth;
	private double Lprev = 0, Rprev = 0, Angprev = 0;
	private double Xpos = 0, Ypos = 0, angle = 0;
	/*
	 * Position accumulation class constructor.
	 *
	 * 
	 */

	public PositionAccumulator(Encoder L, Encoder R, Gyro g, double trackWidth) {
		left = L;
		right = R;
		this.trackWidth = (float) trackWidth;
		gyro = g ; //added STJ
	}

	public void setStartPosition(double Xinches, double Yinches) {
		Xpos = Xinches;
		Ypos = Yinches;
		Lprev = 0;
		Rprev = 0;
		angle = 0;
		Angprev = 0;
	}

	public double[] getPositionInFeet() {
		double[] feetPosition = {Xpos / 12, Ypos / 12, angle * 180 / Math.PI};
		return feetPosition;
	}

	public double[] getPosition() {
		double[] inchPosition = {Xpos, Ypos, angle * 180 / Math.PI};
		return inchPosition;
	}

	double chgX, chgY;
//	public double getDL(){
//		return chgL;
//	}
//	public double getDR(){
//		return chgR;
//	}
	double Lnow = 0, Rnow = 0;
	double dX = 0, dY = 0;

	public void updatePosition() {
		Lprev = Lnow;
		Rprev = Rnow;
		Angprev = angle ;
		Lnow = left.getDistance();
		Rnow = right.getDistance();
		angle = gyro.getAngle() ; //added STJ
		double dL = Lnow - Lprev;
		double dR = Rnow - Rprev;
		double dAngle = angle - Angprev;
		//double dAngle = (dL - dR) / trackWidth;

// the threshold for this test should be less than 1/(ticksPerInch*trackWidth)
// which in our case is about 0.003.  Set it to maybe half that.
//		if (dL != dR && Math.abs(dAngle) > .0002) {
		if (Math.abs(dR-dL)>0.03 && Math.abs(dAngle) > 0.00005) {
			double radius = 0.5 * (dL + dR) / (dL - dR) * trackWidth;
			double chordLength = 2.0 * radius * Math.sin(dAngle / 2.0);
			dX = chordLength * Math.sin(dAngle / 2.0);
			dY = chordLength * Math.cos(dAngle / 2.0);
		}
		else {
			dX = 0.0 ;
			dY = (dR + dL) / 2.0 ;
			dAngle = 0.0 ; 
		}
		// now convert to field coordinates and accumulate values
//		angle += dAngle;
		Xpos += dX * Math.cos(angle) + dY * Math.sin(angle);
		Ypos += -dX * Math.sin(angle) + dY * Math.cos(angle);
		chgX = dX;
		chgY = dY;
	}
}
